#include "RMRAuto.h"
#include "WPILib.h"
/*
 * RMRAuto.cpp
 * 
 * Definition of RMRAuto class
 * 
 * Manages autonomous code
 */


// constructor
RMRAuto::RMRAuto(Watchdog &watchdog, RMRDrive &rmrDrive):
m_watchdog(watchdog), 
m_rmrDrive(rmrDrive),
m_ds(NULL),
m_auto_code_1(SIDECAR_1, AUTO_MODE_0),
m_auto_code_2(SIDECAR_1, AUTO_MODE_1)

{
	m_ds = DriverStation::GetInstance();
	
}


	
//Get the autonomous setting
int RMRAuto::GetAutoSelect()
{
	int button1 = m_auto_code_1.Get();
	int button2 = m_auto_code_2.Get();
	
	int result = button1 << 1 + button2;
	return result;
}
		
//Initialize hardware we need before autonomous
void RMRAuto::initHardware()
{
	//Calibrate steering encoder
	//m_rmrDrive.CalibrateSteering();
	//Calibrate accelerometer
	
	// start air compressor?
	
}

// drive straight at the given speed (in wheel rotation per second) for the given time (in seconds)
//Returns either when time is up or on exiting autonomous mode
void RMRAuto::DriveStraight(float speed, float time)
{
	printf("drive straight called with these args speed = %f time = %f", speed, time);
	m_timer.Start();
	while(m_timer.Get() < time && m_ds->IsAutonomous())
	{
		m_watchdog.Feed();
		m_rmrDrive.Car_Drive(speed, 0);
	}
	m_timer.Stop();

}

//Turn at the given turn(in terms of -1.0 = left, 1.0 = right) at the given speed (in wheel rotation per second) for the given time (in seconds)
//Returns either when time is up or on exiting autonomous mode
void RMRAuto::Turn(float turn, float speed, float time)
{
	printf("turn called with these args radius = %f speed = %f time = %f",  turn, speed, time);
	m_timer.Start();
	while(m_timer.Get() < time && m_ds->IsAutonomous())
	{
		m_watchdog.Feed();
		m_rmrDrive.Car_Drive(speed, turn);
	}
	m_timer.Stop();
	
}

//Pause for the given time in seconds, Returns either when time is up or on exiting autonomous mode
void RMRAuto::Pause(float time)
{
	printf("pause called with these args time = %f", time);
	m_timer.Start();
	while(m_timer.Get() < time && m_ds->IsAutonomous())
	{
		m_watchdog.Feed();
	}
	m_timer.Stop();
}

